#include "stdafx.h"
#include "Conversion.h"
#include <cmath>
#include <iostream>
using namespace std;

		Quaternion AxisAngleToQuaternion(float angle,float x,float y,float z)
		{
			angle=(angle*M_PI)/180.0f;
			float s=sin(angle/2.f);
			float xx=cos(angle/2.f);
			return Quaternion(xx,s*x,s*y,s*z);
		}

		float* QuaternionToAxisAngle(Quaternion &a)
		{
			float* temp=a.GetArray();
			if (abs(temp[0])> 1.0f) {temp=a.Norm().GetArray();}
			

			float angle=2.0f * acos(temp[0]);
			float s=sqrt(1.0f-temp[0]*temp[0]);

			float *xyz = (float*) _aligned_malloc(4 * sizeof(float), 16);
			if (s < 0.001f)
			{
				
				xyz[0]=angle*(180.0f/M_PI);
				xyz[1]=temp[1];
				xyz[2]=temp[2];
				xyz[3]=temp[3];
			}
			else
			{
				
				xyz[0]=angle*(180.0f/M_PI);
				xyz[1]=temp[1]/s;
				xyz[2]=temp[2]/s;
				xyz[3]=temp[3]/s;
			}
			cout<<"angle: "<<xyz[0]<<" x: "<<xyz[1]<<" y: "<<xyz[2]<<" z: "<<xyz[3]<<endl;
			return xyz;
		}

		float QuaternionToAngle(float scalar)
		{
			float angle=2.0f * acos(scalar);
			float degreeAngle=angle*(180.0f/M_PI);

			return degreeAngle;		
		}

		float* QuaternionsToEuler(Quaternion &a) //ToDO
		{
		   float* temp=a.GetArray();
		   float test=temp[1]*temp[2]+temp[3]*temp[0];  

		   return temp;		
		}


		Quaternion EulerToQuaternion(float pitch,float yaw,float roll)
		{
			pitch=(pitch*M_PI)/180.0f;
			yaw=(yaw*M_PI)/180.0f;
			roll=(roll*M_PI)/180.0f;

			
			float c1 = cos(yaw/2);
			float s1 = sin(yaw/2);
			float c2 = cos(pitch/2);
			float s2 = sin(pitch/2);
			float c3 = cos(roll/2);
			float s3 = sin(roll/2);

			return Quaternion(c1*c2*c3-s1*s2*s3,s1*s2*c3+c1*c2*s3,s1*c2*c3+c1*s2*s3,c1*s2*c3-s1*c2*s3);
		}